#include "stereo_capture.h"
#include <iostream>

#define FRAME_WIDTH	1280
#define FRAME_HEIHGT 480

using namespace std;
using namespace cv;


static void sleepMs(uint32_t timeMs)
{
	usleep(timeMs * 1000);
}


static long long getNanosecondsTimestamp()
{
    auto now = std::chrono::system_clock::now();
    auto duration = now.time_since_epoch();
    return std::chrono::duration_cast<std::chrono::nanoseconds>(duration).count();
}



StereoCapture::StereoCapture():
	m_leftVideoCallbackFunction(NULL),
	m_rightVideoCallbackFunction(NULL),
	m_width(0),
	m_height(0)
{

}

StereoCapture::~StereoCapture()
{

}


bool StereoCapture::registerStereoLeftVideoCallback(stereoLeftVideoCallback function)
{
	if(m_leftVideoCallbackFunction != NULL)
	{
		return false;
	}

	m_leftVideoCallbackFunction = function;
	return true;
}


bool StereoCapture::registerStereoRightVideoCallback(stereoRightVideoCallback function)
{
	if(m_rightVideoCallbackFunction != NULL)
	{
		return false;
	}

	m_rightVideoCallbackFunction = function;
	return true;
}

//vmware set usb 3.1, opencv 4
bool StereoCapture::startVideoCapture()
{
	//FRAME_WIDTH  1280 ,FRAME_HEIHGT 480
	std::string ffmpegCmd = "v4l2src device=/dev/video0 ! "
                           "image/jpeg,width=1280,height=480 ! "
                           "jpegdec ! videoconvert ! appsink";
	cv::VideoCapture capture(ffmpegCmd, cv::CAP_GSTREAMER);

	if (!capture.isOpened())
	{
		cout << "open capture error" << endl;
		return false;
	}

	m_width = capture.get(cv::CAP_PROP_FRAME_WIDTH);
	m_height = capture.get(cv::CAP_PROP_FRAME_HEIGHT);
	m_fps = capture.get(cv::CAP_PROP_FPS);

	int mjpgCodec = cv::VideoWriter::fourcc('M', 'J', 'P', 'G');
	int yuy2Code = cv::VideoWriter::fourcc('Y', 'U', 'Y', '2');

	int code = capture.get(cv::CAP_PROP_FOURCC);

	double interval = 1.0 / m_fps;

	cout << "width:" << m_width << ",height:" << m_height << ",fps:" << m_fps << endl;

	if(code == mjpgCodec)
	{
		cout << "fourcc:MJPG" << endl;
	}
	else if(code == yuy2Code)
	{
		cout << "fourcc:YUY2" << endl;
	}
	else
	{
		cout << "fourcc:" << code << endl;
	}

	Mat frame, left, right;
	long long captureTimesStamp;
	while(1)
	{
		std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

		capture >> frame;
		captureTimesStamp = getNanosecondsTimestamp();
		if (nullptr == frame.ptr() || 0 == frame.rows || 0 == frame.cols)
		{
			//restart.
			capture.release();
			cv::VideoCapture capture(ffmpegCmd, cv::CAP_GSTREAMER);
			sleepMs(100);
			continue;
		}

		Rect leftPosition(0, 0, FRAME_WIDTH / 2, FRAME_HEIHGT);
		left = (Mat(frame, leftPosition)).clone();

		Rect rightPosition(FRAME_WIDTH / 2, 0, FRAME_WIDTH / 2, FRAME_HEIHGT);
		right = (Mat(frame, rightPosition)).clone();


		//callback
		if(m_leftVideoCallbackFunction != NULL)
		{
			(*m_leftVideoCallbackFunction)(left, captureTimesStamp);
		}

		if(m_rightVideoCallbackFunction != NULL)
		{
			(*m_rightVideoCallbackFunction)(right, captureTimesStamp);
		}

		std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();

		double ttrack = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count();
		//cout << "ttrack:" << ttrack * 1e3  << endl;

		// Wait to load the next frame
		int delay = (interval - ttrack) * 1e3;
		delay = delay <= 0 ? 1 : delay;
#if 0
		imshow("video", frame);
		imshow("left", left);
		imshow("right", right);
		waitKey(delay);
#else
		sleepMs(1);
#endif
	}

	capture.release();

	return true;
}


StereoCapture StereoCaptureHandler;
